/************************************************************************
* math_matrix.c
* voxelands - 3d voxel world sandbox game
* Copyright (C) Lisa 'darkrose' Milne 2016 <lisa@ltmnet.com>
*
* This program is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
* See the GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this program.  If not, see <http://www.gnu.org/licenses/>
************************************************************************/

#include "common.h"

#include <math.h>

/* initialise an identity matrix */
void matrix_init(matrix_t *m)
{
	int i;
	for (i=0; i<16; i++) {
		m->data[i] = 0.0;
	}
	m->data[0] = 1.0;
	m->data[5] = 1.0;
	m->data[10] = 1.0;
	m->data[15] = 1.0;
}

/* create an identity matrix */
matrix_t *matrix_create()
{
	matrix_t *m = malloc(sizeof(matrix_t));

	matrix_init(m);

	return m;
}

/* set the values of a matrix */
void matrix_set(
	matrix_t *m,
	float m0,
	float m1,
	float m2,
	float m3,
	float m4,
	float m5,
	float m6,
	float m7,
	float m8,
	float m9,
	float m10,
	float m11,
	float m12,
	float m13,
	float m14,
	float m15
)
{
	m->data[0] = m0;
	m->data[1] = m1;
	m->data[2] = m2;
	m->data[3] = m3;
	m->data[4] = m4;
	m->data[5] = m5;
	m->data[6] = m6;
	m->data[7] = m7;
	m->data[8] = m8;
	m->data[9] = m9;
	m->data[10] = m10;
	m->data[11] = m11;
	m->data[12] = m12;
	m->data[13] = m13;
	m->data[14] = m14;
	m->data[15] = m15;
}

/* multiply a matrix by another matrix */
void matrix_multiply(matrix_t *m, matrix_t *mul)
{
	int i;
	float md[16];

	md[0] = (m->data[0]*mul->data[0])+(m->data[4]*mul->data[1])+(m->data[8]*mul->data[2])+(m->data[12]*mul->data[3]);
	md[1] = (m->data[1]*mul->data[0])+(m->data[5]*mul->data[1])+(m->data[9]*mul->data[2])+(m->data[13]*mul->data[3]);
	md[2] = (m->data[2]*mul->data[0])+(m->data[6]*mul->data[1])+(m->data[10]*mul->data[2])+(m->data[14]*mul->data[3]);
	md[3] = (m->data[3]*mul->data[0])+(m->data[7]*mul->data[1])+(m->data[11]*mul->data[2])+(m->data[15]*mul->data[3]);

	md[4] = (m->data[0]*mul->data[4])+(m->data[4]*mul->data[5])+(m->data[8]*mul->data[6])+(m->data[12]*mul->data[7]);
	md[5] = (m->data[1]*mul->data[4])+(m->data[5]*mul->data[5])+(m->data[9]*mul->data[6])+(m->data[13]*mul->data[7]);
	md[6] = (m->data[2]*mul->data[4])+(m->data[6]*mul->data[5])+(m->data[10]*mul->data[6])+(m->data[14]*mul->data[7]);
	md[7] = (m->data[3]*mul->data[4])+(m->data[7]*mul->data[5])+(m->data[11]*mul->data[6])+(m->data[15]*mul->data[7]);

	md[8] = (m->data[0]*mul->data[8])+(m->data[4]*mul->data[9])+(m->data[8]*mul->data[10])+(m->data[12]*mul->data[11]);
	md[9] = (m->data[1]*mul->data[8])+(m->data[5]*mul->data[9])+(m->data[9]*mul->data[10])+(m->data[13]*mul->data[11]);
	md[10] = (m->data[2]*mul->data[8])+(m->data[6]*mul->data[9])+(m->data[10]*mul->data[10])+(m->data[14]*mul->data[11]);
	md[11] = (m->data[3]*mul->data[8])+(m->data[7]*mul->data[9])+(m->data[11]*mul->data[10])+(m->data[15]*mul->data[11]);

	md[12] = (m->data[0]*mul->data[12])+(m->data[4]*mul->data[13])+(m->data[8]*mul->data[14])+(m->data[12]*mul->data[15]);
	md[13] = (m->data[1]*mul->data[12])+(m->data[5]*mul->data[13])+(m->data[9]*mul->data[14])+(m->data[13]*mul->data[15]);
	md[14] = (m->data[2]*mul->data[12])+(m->data[6]*mul->data[13])+(m->data[10]*mul->data[14])+(m->data[14]*mul->data[15]);
	md[15] = (m->data[3]*mul->data[12])+(m->data[7]*mul->data[13])+(m->data[11]*mul->data[14])+(m->data[15]*mul->data[15]);

	for (i=0; i<16; i++) {
		m->data[i] = md[i];
	}
}

/* translate a matrix */
void matrix_translate(matrix_t *m, float x, float y, float z)
{
	m->data[0] += m->data[3] * x;
	m->data[4] += m->data[7] * x;
	m->data[8] += m->data[11] * x;
	m->data[12] += m->data[15] * x;
	m->data[1] += m->data[3] * y;
	m->data[5] += m->data[7] * y;
	m->data[9] += m->data[11] * y;
	m->data[13] += m->data[15] * y;
	m->data[2] += m->data[3] * z;
	m->data[6] += m->data[7] * z;
	m->data[10] += m->data[11] * z;
	m->data[14] += m->data[15] * z;
}

/* translate a matrix by a vector */
void matrix_translate_v(matrix_t *m, v3_t *v)
{
	matrix_translate(m,v->x,v->y,v->z);
}

/* scale a matrix */
void matrix_scale(matrix_t *m, float x, float y, float z)
{
	m->data[0] *= x;
	m->data[4] *= x;
	m->data[8] *= x;
	m->data[12] *= x;
	m->data[1] *= y;
	m->data[5] *= y;
	m->data[9] *= y;
	m->data[13] *= y;
	m->data[2] *= z;
	m->data[6] *= z;
	m->data[10]*= z;
	m->data[14] *= z;
}

/* scale a matrix by a vector */
void matrix_scale_v(matrix_t *m, v3_t *v)
{
	matrix_scale(m,v->x,v->y,v->z);
}

/* rotate a matrix by axis values */
void matrix_rotate(matrix_t *m, float rads, float x, float y, float z)
{
	int i;
	float c;
	float s;
	float c1;
	float md[16];
	float r[11];

	for (i=0; i<16; i++) {
		md[i] = m->data[i];
	}

	c = cosf(rads);
	s = sinf(rads);
	c1 = 1.0 - c;

	r[0] = x * x * c1 + c;
	r[1] = x * y * c1 + z * s;
	r[2] = x * z * c1 - y * s;
	r[4] = x * y * c1 - z * s;
	r[5] = y * y * c1 + c;
	r[6] = y * z * c1 + x * s;
	r[8] = x * z * c1 + y * s;
	r[9] = y * z * c1 - x * s;
	r[10]= z * z * c1 + c;

	m->data[0] = r[0] * md[0] + r[4] * md[1] + r[8] * md[2];
	m->data[1] = r[1] * md[0] + r[5] * md[1] + r[9] * md[2];
	m->data[2] = r[2] * md[0] + r[6] * md[1] + r[10]* md[2];
	m->data[4] = r[0] * md[4] + r[4] * md[5] + r[8] * md[6];
	m->data[5] = r[1] * md[4] + r[5] * md[5] + r[9] * md[6];
	m->data[6] = r[2] * md[4] + r[6] * md[5] + r[10]* md[6];
	m->data[8] = r[0] * md[8] + r[4] * md[9] + r[8] * md[10];
	m->data[9] = r[1] * md[8] + r[5] * md[9] + r[9] * md[10];
	m->data[10]= r[2] * md[8] + r[6] * md[9] + r[10]* md[10];
	m->data[12]= r[0] * md[12]+ r[4] * md[13]+ r[8] * md[14];
	m->data[13]= r[1] * md[12]+ r[5] * md[13]+ r[9] * md[14];
	m->data[14]= r[2] * md[12]+ r[6] * md[13]+ r[10]* md[14];
}

/* rotate a matrix by axis vector values */
void matrix_rotate_v(matrix_t *m, float rads, v3_t *v)
{
	matrix_rotate(m,rads,v->x,v->y,v->z);
}

/* rotate by radians around the x axis */
void matrix_rotate_x(matrix_t *m, float rads)
{
	int i;
	float c;
	float s;
	float md[16];

	for (i=0; i<16; i++) {
		md[i] = m->data[i];
	}

	c = cosf(rads);
	s = sinf(rads);

	m->data[1] = md[1] * c + md[2] *-s;
	m->data[2] = md[1] * s + md[2] * c;
	m->data[5] = md[5] * c + md[6] *-s;
	m->data[6] = md[5] * s + md[6] * c;
	m->data[9] = md[9] * c + md[10]*-s;
	m->data[10]= md[9] * s + md[10]* c;
	m->data[13]= md[13]* c + md[14]*-s;
	m->data[14]= md[13]* s + md[14]* c;
}

/* rotate by radians around the x axis */
void matrix_rotate_y(matrix_t *m, float rads)
{
	int i;
	float c;
	float s;
	float md[16];

	for (i=0; i<16; i++) {
		md[i] = m->data[i];
	}

	c = cosf(rads);
	s = sinf(rads);

	m->data[0] = md[0] * c + md[2] * s;
	m->data[2] = md[0] *-s + md[2] * c;
	m->data[4] = md[4] * c + md[6] * s;
	m->data[6] = md[4] *-s + md[6] * c;
	m->data[8] = md[8] * c + md[10]* s;
	m->data[10]= md[8] *-s + md[10]* c;
	m->data[12]= md[12]* c + md[14]* s;
	m->data[14]= md[12]*-s + md[14]* c;
}

/* rotate by radians around the x axis */
void matrix_rotate_z(matrix_t *m, float rads)
{
	int i;
	float c;
	float s;
	float md[16];

	for (i=0; i<16; i++) {
		md[i] = m->data[i];
	}

	c = cosf(rads);
	s = sinf(rads);

	m->data[0] = md[0] * c + md[1] *-s;
	m->data[1] = md[0] * s + md[1] * c;
	m->data[4] = md[4] * c + md[5] *-s;
	m->data[5] = md[4] * s + md[5] * c;
	m->data[8] = md[8] * c + md[9] *-s;
	m->data[9] = md[8] * s + md[9] * c;
	m->data[12]= md[12]* c + md[13]*-s;
	m->data[13]= md[12]* s + md[13]* c;
}

/* rotate a matrix by axis values - degrees version */
void matrix_rotate_deg(matrix_t *m, float degrees, float x, float y, float z)
{
	float rads = math_degrees_to_radians(degrees);
	matrix_rotate(m,rads,x,y,z);
}

/* rotate a matrix by axis vector values - degrees version */
void matrix_rotate_deg_v(matrix_t *m, float degrees, v3_t *v)
{
	float rads = math_degrees_to_radians(degrees);
	matrix_rotate(m,rads,v->x,v->y,v->z);
}

/* rotate by degrees around the x axis */
void matrix_rotate_deg_x(matrix_t *m, float degrees)
{
	float rads = math_degrees_to_radians(degrees);
	matrix_rotate_x(m,rads);
}

void matrix_rotate_deg_y(matrix_t *m, float degrees)
{
	float rads = math_degrees_to_radians(degrees);
	matrix_rotate_y(m,rads);
}

void matrix_rotate_deg_z(matrix_t *m, float degrees)
{
	float rads = math_degrees_to_radians(degrees);
	matrix_rotate_z(m,rads);
}

/* rotate a matrix by a quaternion */
void matrix_rotate_quat(matrix_t *m, quaternion_t *q)
{
	v3_t v;
	float r;
	/* TODO: this might get degrees, so use the degree version of the next function */
	quat_get_axis(q,&v,&r);
	matrix_rotate_v(m,r,&v);
}
